#include "graphicphysicbridge.h"

#include <assert.h>

//Merci Stefan !
void static QuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler) {
    btScalar W = TQuat.getW();
    btScalar X = TQuat.getX();
    btScalar Y = TQuat.getY();
    btScalar Z = TQuat.getZ();
    float WSquared = W * W;
    float XSquared = X * X;
    float YSquared = Y * Y;
    float ZSquared = Z * Z;

    TEuler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
    TEuler.setY(asinf(-2.0f * (X * Z - Y * W)));
    TEuler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
    TEuler *= irr::core::RADTODEG;
}

GraphicPhysicBridge::GraphicPhysicBridge(const btTransform &initialPosition, irr::scene::ISceneNode *node)
    : m_node(node)
    , m_position(initialPosition)
{

}

GraphicPhysicBridge::~GraphicPhysicBridge(void)
{

}

void GraphicPhysicBridge::getWorldTransform(btTransform &worldTrans) const
{
    worldTrans = m_position;
}

void GraphicPhysicBridge::setWorldTransform(const btTransform &worldTrans)
{
    //No, I really want to have a node !
    //if(NULL == m_node) return; // silently return before we set a node
    assert( m_node != NULL );

    btQuaternion rot = worldTrans.getRotation();
    btVector3 pos = worldTrans.getOrigin();

    m_node->setPosition(
                irr::core::vector3df(
                    irr::f32(pos.x()),
                    irr::f32(pos.y()),
                    irr::f32(pos.z())) );

    btVector3 eulerRotation;
    QuaternionToEuler(rot, eulerRotation);
    m_node->setRotation(
                irr::core::vector3df(
                    irr::f32(eulerRotation.x()),
                    irr::f32(eulerRotation.y()),
                    irr::f32(eulerRotation.z())) );
}

BridgeFactory::BridgeFactory(GraphicEngine& engine)
	: MotionStateFactory()
	, m_engine_reference(engine)
	, m_id_generator(0)
{
}

btMotionState* BridgeFactory::create(
	const btTransform& initialPosition,
	const btVector3&   initialDimension)
{
	irr::scene::ISceneManager& scene = m_engine_reference.scene_manager();

	irr::scene::ISceneNode* scene_node = scene.addCubeSceneNode(1.0f);
	scene_node->setScale(
		irr::core::vector3df(
		irr::f32( initialDimension.x() ),
		irr::f32( initialDimension.y() ),
		irr::f32( initialDimension.z() )));

	scene_node->setMaterialFlag( irr::video::EMF_LIGHTING, 1);
	scene_node->setMaterialFlag( irr::video::EMF_NORMALIZE_NORMALS, true);
	scene_node->setID( ++m_id_generator );	

	return new GraphicPhysicBridge(initialPosition, scene_node);
}
